#include "C_H.h"
#include "meun.h"
// 全局变量定义
//*******************************************************

//无用残留变量
float Weighting[10] = {0.96, 0.92, 0.88, 0.83, 0.77, 0.71, 0.65, 0.59, 0.53, 0.47}; // 10行权重参数

//opencv相关
cv::VideoCapture cap;
cv::Mat frame, gray_frame, compressed;                                              // 定义为全局变量
int sock;                                                                           // 添加socket全局变量定义

//帧率计算
float fps = 0.0f;

// 舵机控制
float pid_output;
int image_error;
uint8_t mohu_flag=0;
int servo_duty;
uint16_t servo_base = 763;//5240;//4440;//886;  
int wall_first=0,wall_L,wall_R;

// 电机控制
float setPoint = 0.0; // 设定点
//目标速度
int huandao_speed = 560, lz_speed = 700, dz_speed = 500, wan_speed = 540,ramp_speed_up=400,ramp_speed_down=600;
int set_speed_low[6] = {0, 0, 0, 0, 0, 0}; // 设置低速模式下的速度
int set_speed_mid[6] = {0, 0, 0, 0, 0, 0}; // 设置中速模式下的速度
int set_speed_high[6] = {0, 0, 0, 0, 0, 0}; // 设置高速模式下的速度
//差速
float raw_output_l;
float raw_output_r;
uint16_t chasu_xianfu=2000;

//打角
uint8_t dajiao = 15;
uint8_t dj = 35;
uint8_t up_dj = 39;
uint8_t down_dj = 34;

//运行程序相关变量
uint8_t tiaoshi_flag = 0; // 调试标志
uint8_t exit_flag=0;
uint8_t getDet_flag=1;
uint64_t elapsed = 0;
uint64_t elapsed_distance = 0;
uint64_t all_start_time = 0;  // 系统启动时间戳（毫秒）
uint64_t all_start_distance = 0; // 系统启动时的行驶距离（毫米）
uint64_t jishi_start_time = 0; 
uint64_t during_time = 0;  // 系统启动时间戳（毫秒）
float distance = 0;
float speed_jisuan = 0;
float chasu_k = 1.5;

// 斑马线控制
uint8_t banma_fache_flag = 0; 
uint8_t speed_test_mode = 0; 
bool has_been_banmaxian = false;  // 标记是否已经经历过斑马线

// 保护
uint8_t switch_baohu = 1;
float time_lock = 5500; // 时间锁
byte banma_jieshu_cnt = 2;

// 常量定义
float HIGH_SPEED_THRESHOLD = 800.0f; // 区分高速/低速的阈值（根据实际情况调整）
//****************************图像处理****************************
uint otsuThreshold;
uint8_t BinPixels[LCDH][LCDW];

//路道状态
uint8_t road_state = 3;
int flag_break = 0;
int cross_state_flag = 0;

//图像辅助
float parameterk, parameterb;
int16 parameterx;
uint8 L_Start[MT9V03X_H];
uint8 R_Start[MT9V03X_H];


// 大津法
uint otsuMinThreshold = 100;   // 默认下限
uint otsuMaxThreshold = 255; // 默认上限

// 扫线
uint8_t baseline_l = 93; // 上一次基准线列
uint8_t L_black[75], R_black[75], LCenter[75]; // 左边界和右边界
uint8_t rightflag[70] = {0};                   // 右线丢线标志
uint8_t leftflag[70] = {0};                    // 左线丢线标志

// 直弯分离
uint8_t wan_cnt = 0;
uint8_t Road_flag = 0;
const float straight_standard1 = 1.75f; // 直线判定阈值
int tixing_start = 15, tixing_end = 52;
int tixing_mid = 186;
int tixing_cha = 65;//79;
// 直弯判断——新
float Ey_mid, Ey2_mid, Dy_mid;
int mid_list_sum, mid_list_sum2;
float mid_list_k;
int list_lenth = 6;
uint8_t Road_flag_last = 0;
uint16_t Road_flag_cnt = 0;

//**********************************************************************图像处理**********************************************************************


//基础变量
byte xielv_use;
byte qidian = 93;
byte hang_max = 0;
byte start_lie;
byte last_lie;
byte lie_zhong;
int lie;
byte hang;
float sumE;
float parameterA, parameterB;
float curve_a, curve_b;
uint8_t R_expect[70];
uint8_t L_expect[70];
uint8_t record_leftline_begin[10];
uint8_t record_leftline_end[10];
uint8_t record_rightline_begin[10];
uint8_t record_rightline_end[10];

double lie_xielv[26] = {
    0.21255, 0.32491, 0.44522, 0.57735, 0.72654, 0.90040, 1.11061,
    1.37638, 1.73205, 2.24603, 3.07768, 4.70463, 9.51436, -9.51436,
    -4.70463, -3.07768, -2.24603, -1.73205, -1.37638, -1.11061,
    -0.90040, -0.72654, -0.57735, -0.44522, -0.32491, -0.21255
};

byte reference_lie[26][70];  
byte guai_point_huandao[4][2]; 

byte baseline_use;
byte lie_xielv_use_begin = 0;  // Slope reference line storage
byte lie_xielv_use_end = 26;

//环岛
byte huandao_flag_right = 0;
byte huandao_flag_left = 0;
byte wr_cnt_left = 0;
byte wr_cnt_right = 0;
byte mirror_LCenter = 93;
byte huandao_flag_3_use = 0;
byte huandao_flag_3_use_need = 40;
byte huandao_out_point_lie = 0;
byte state_7_guaidian_flag = 0;
float fangcha_zuo = 0;  // Roundabout left line variance
float fangcha_you = 0;  // Roundabout right line variance
byte left_break;        // Left break
byte right_break;       // Right break

byte huandao_point_old[2];
//内部
byte guai_point_zhong_pd[4][2];
//拐点
byte guai_point_real[4][2];
byte last_real_ys_x = 0;
byte last_real_ys_y = 0;
byte last_real_zs_x = 0;
byte last_real_zs_y = 0;
byte left_point_up_flag = 0;
byte right_point_up_flag = 0;
//十字
byte shizi_use = 0;
byte zr_shizi_flag = 0;
byte xie_shizi_flag_right = 0;
byte xie_shizi_flag_left = 0;
byte zr_shizi_now_flag = 0;

double hang_xielv_use_now = 0;
byte lie_max = 0;
byte lie_zhong_left = 0;
byte lie_zhong_right = 0;
int lie_len_max = 0;
byte reference_hang[186];
byte left_guaidian_no_use = 0;
byte right_guaidian_no_use = 0;
byte hang_x_point_no_use_flag_left = 0;
byte hang_x_point_no_use_flag_right = 0;
byte right_up_point_flag = 0;
byte left_up_point_flag = 0;

//
byte shizi_break_right = 0;//十字断线判断
byte shizi_break_left = 0;
byte shizi_break_right_hang = 0;
byte shizi_break_left_hang = 0;
//
byte duanzhi_shizi_you = 0;
byte duanzhi_shizi_zuo = 0;
//内部
byte guai_point_get[4][2];
byte guai_point_ys[2] = {0, 0};  //行  列
byte guai_point_yx[2] = {0, 0};  //行  列
byte guai_point_zs[2] = {0, 0};  //行  列
byte guai_point_zx[2] = {0, 0};  //行  列
//斑马线
byte banmaxian_flag = 0;//斑马线标志1：斑马线状态 0：非斑马线状态
byte banma_cnt = 0;//遇到斑马线次数
byte banma_up_hang = 0;
byte banma_dinghang;//斑马线最顶行
byte banma_dihang;//斑马线最底行
byte baseline;//斑马线扫线
byte banma_tb = 0;//斑马线跳变
/**************坡道相关变量************/
float average0 = 0, k_LCenter = 0, s_LCenter = 0;
float average1 = 0, k_L_black = 0, s_L_black = 0;
float average2 = 0, k_R_black = 0, s_R_black = 0;
int ramp_flag = 0;//坡道状态
int ramp_protect_time = 0;//坡道保护时间
int ramp_top = 70;//坡道顶行
int continuity_hang = 0;//坡道连续行
int ramp_cnt = 0;
int ramp_number;
byte ramp_width;
/**************路障相关变量************/
byte barrier_flag;
byte barrier_flag_left;
byte barrier_flag_right;
byte wide[60]={0};
byte barrier_no_use_flag = 0;
/**************小s相关变量************/
byte Little_s_flag = 0;
byte end_hang = 0;
double t;
double PP[4];
int Line[70];
//Find_Baseline_lie
double hang_xielv[19]={0};
byte hang_left_begin[10]={0};
byte hang_lost_left_begin = 0;
byte hang_left_end[10]={0};
byte hang_lost_left_end = 0;
byte hang_right_begin[10]={0};
byte hang_lost_right_begin = 0;
byte hang_right_end[10]={0};
byte hang_lost_right_end = 0;
byte hang_lost_left_cnt = 0;
byte hang_lost_right_cnt = 0;
int lie_len = 0;
byte hang_xielv_use = 0;
byte flag_begin = 0;

//**********************************************************************图像处理**********************************************************************

//王cl图像定义
byte baseline_last,endline;           
uint8 right_break_flag[MT9V03X_H];
uint8 left_break_flag[MT9V03X_H];
byte ob_cross_flag,cross_flag;
byte Up_limit,left_D,right_D;



// 开关定义
uint8_t zhushi_flag = 0;
uint8_t serve_flag = 1;
uint8_t run_flag = 0;
uint8_t sd_flag = 1;
uint8_t break_flag = 0;
uint8_t chasu_flag = 1; // 刹车标志1
uint8_t dj_flag = 1;
uint8_t fps_flag = 0;

void guai_point_8(byte type , byte starthang);
void saoxian_shu();
void run_init(void);
void Image_Process();

void Find_Baseline_hang();
void Find_Baseline();
void Baseline_Findline();
void Calculate_Slope(byte type, byte starthang, byte endhang);
void Bezier_get();
double hang_xielv_x_f(double value);  // 函数声明

bool initCamera(void);
void ImageProcess(void);
uint8_t findBaseline(void);
void saoxian(void);
void applyThresholdWithLimits(cv::Mat &src, cv::Mat &dst);

#include <time.h>
#include <stdint.h>

// 获取系统当前时间（毫秒）
uint64_t get_current_time_ms() {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);  // 使用单调时钟避免系统时间跳变
    return (uint64_t)ts.tv_sec * 1000 + (uint64_t)ts.tv_nsec / 1000000;
}


// 初始化摄像头
bool initCamera()
{
    cap.open(0);
    if (!cap.isOpened())
    {
        std::cerr << "错误：无法打开摄像头" << std::endl;
        return false;
    }


    // 设置摄像头参数为160*120
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 160);  
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 120); 
    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    cap.set(cv::CAP_PROP_FPS, 120);

    // 获取摄像头图像宽高和帧率
    int frame_width = cap.get(cv::CAP_PROP_FRAME_WIDTH);
    int frame_height = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
    double frame_fps = cap.get(cv::CAP_PROP_FPS);
    printf("frame:%d*%d, fps:%3f\n\r", frame_width, frame_height, frame_fps);
    sleep(1);

    // // 预分配内存
    // frame = cv::Mat(120, 160, CV_8UC3);      // 更新为320x240
    // gray_frame = cv::Mat(120, 160, CV_8UC1); // 更新为320x240
    // compressed = cv::Mat(LCDH, LCDW, CV_8UC1);

    return true;
}

#ifdef erdui
// 裁剪图像的函数
cv::Mat cropImage(const cv::Mat &input_frame)
{
    // 裁剪图像，去掉上面50行、下面25行、左侧30列和右侧30列
    int top = 40;        // 去掉上面40行
    int bottom = 39;     // 去掉下面40行
    int left = 26;       // 新增：去掉左侧30列
    int right = 26;      // 新增：去掉右侧30列
    
    // 定义裁剪区域（调整宽度和起始列）
    cv::Rect roi(left, top, 
                 input_frame.cols - left - right,  // 调整宽度
                 input_frame.rows - top - bottom); // 调整高度
    
    cv::Mat cropped_frame = input_frame(roi);     // 裁剪图像

    // 将图像缩放为186x70
    cv::Mat resized_frame;
    cv::resize(cropped_frame, resized_frame, cv::Size(186, 70)); // 宽度变为186，高度变为70

    return resized_frame; // 返回裁剪和缩放后的图像
}
#endif


// 修改后的二值化处理函数
void applyThresholdWithLimits(cv::Mat &src, cv::Mat &dst)
{
    double otsuValue = cv::threshold(src, gray_frame, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);

    otsuThreshold = static_cast<uint>(otsuValue);
    otsuThreshold = std::max(otsuMinThreshold, otsuThreshold); // 不低于下限
    otsuThreshold = std::min(otsuMaxThreshold, otsuThreshold); // 不超过上限

    // Step 3: 用修正后的阈值二值化图像
    cv::threshold(src, dst, otsuThreshold, 255, cv::THRESH_BINARY);
}
int ima_change=0;
// 修改后的getCompressedImage函数
void getCompressedImage()
{
    if (!cap.read(frame))
    {
        std::cerr << "错误：无法读取图像" << std::endl;
        return;
    }

    // 检查图像是否有效
    if (frame.empty())
    {
        std::cerr << "错误：读取的图像为空" << std::endl;
        return;
    }

    // 裁剪图像
    frame = cropImage(frame); // 调用裁剪函数

    // 检查裁剪后的图像是否有效
    if (frame.empty())
    {
        std::cerr << "错误：裁剪后的图像为空" << std::endl;
        return;
    }

    // 转换为灰度图
    cv::cvtColor(frame, compressed, cv::COLOR_BGR2GRAY);

    // 进行带限制的二值化处理
    applyThresholdWithLimits(compressed, compressed);
    // otsuThreshold = cv::threshold(compressed, compressed, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);

    if (zhushi_flag == 1)
        std::cout << "OTSU Threshold: " << otsuThreshold << std::endl;

    cv::flip(compressed, compressed, -1); // -1 表示同时水平和垂直翻转

    // 更新BinPixels数组
    for (int i = 0; i < 70; i++)
    {
        for (int j = 0; j < 186; j++)
        {
            assert(i >= 0 && i < 70);  // 检查行索引
            assert(j >= 0 && j < 186); // 检查列索引
            if(ima_change==0)
            BinPixels[69 - i][185 - j] = (compressed.at<uchar>(i, j) > 127) ? 1 : 0;
            else
            BinPixels[69 - i][185 - j] = (compressed.at<uchar>(i, j) > 127) ? 0 : 1;

        }
    }
}

// 主循环
void run_main_loop(void)
{
    while (!should_exit)
    {

        char Key_Num;
        Key_Num = detect_key();
        my_menucmd(Key_Num);
    }
}



void protect(void)
{
    char Key_Num;
    break_flag = 1;
    pwm_set_duty(SERVO_PWM, 0);
    gpio_set_level(BEEP, 0x0);

    #ifdef yidui
    pwm_set_duty(PWM_1, 5000);
    #endif

    #ifdef erdui
    pwm_set_duty(PWM_1, 5000);
    #endif
    
    // 记录进入函数的时间点
    auto protect_start = std::chrono::high_resolution_clock::now();
    bool one_second_passed = false;

    ips200_show_string(0, 20, "max_sp_l");
    ips200_show_string(0, 40, "max_sp_r");
    ips200_show_string(0, 60, "max_sp");


    ips200_show_float(0, 20, max_speed_l, 4, 1);
    ips200_show_float(0, 40, max_speed_r, 4, 1);
    ips200_show_float(0, 60, max_speed, 4, 1);

    char time_str[20];
    snprintf(time_str, sizeof(time_str), "Time: %llu  ms", elapsed);
    ips200_show_string(0, 80, time_str);

    ips200_show_string(0, 100, "speed:");
    ips200_show_float(80, 100, speed_jisuan, 2, 3);
    
    while (!should_exit)
    {
        #ifdef erdui
        pwm_set_duty(PWM_1, 5000);
        #endif
        // 检查是否已经过了0.5秒
        if (!one_second_passed)
        {
            auto current_time = std::chrono::high_resolution_clock::now();
            auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - protect_start);
            
            if (elapsed.count() >= 750) // 1000毫秒=1秒
            {
                run_flag = 0;
                one_second_passed = true;
            }
            else
            {
                saveTask();
            }
        }
        
        Key_Num = detect_key();
        if (Key_Num == 4)
        {
            run_flag = 0;
            break_flag = 0;
            exit_flag = 1;
            break;
        }
    }
    ips200_show_string(0, 0, "                         ");
}

// 误差按权重重新整定
void GetDet()
{
    //计算平均误差
    image_error = (-1) * (LCenter[dajiao] - 93);

    
}
// 修改绘制边界点的函数，直接在彩色缓冲区上绘制
void drawBoundaryPoints()
{
    // 清空显示屏
    ips200_clear();

    // 显示二值化图像
    ips200_show_gray_image(0, 60, (uint8_t *)compressed.data, LCDW, LCDH);

    // 在图像上标记边界点（完全颠倒逻辑）
    for (int row = 0; row < LCDH; row++)
    {
        // 计算颠倒后的行索引
        int flipped_row = LCDH - 1 - row;

        // 计算颠倒后的列索引
        int flipped_L_black = LCDW - 1 - L_black[flipped_row]; // 左边界颠倒
        int flipped_R_black = LCDW - 1 - R_black[flipped_row]; // 右边界颠倒
        int flipped_LCenter = LCDW - 1 - LCenter[flipped_row]; // 中心线颠倒

        // 绘制左边界点（红色）
        ips200_draw_point(flipped_L_black, row + 60, RGB565_RED);

        // 绘制右边界点（蓝色）
        ips200_draw_point(flipped_R_black, row + 60, RGB565_BLUE);

        // 绘制中心线点（绿色）
        ips200_draw_point(flipped_LCenter, row + 60, RGB565_GREEN);
    }

    // 用点绘制方式实现水平线（保持原逻辑不变）
    int screen_y = 60 + (69 - dajiao); // 转换为屏幕坐标（0=最下面，69=最上面）
    for (int x = 0; x < LCDW; x++) {
        ips200_draw_point(x, screen_y, RGB565_YELLOW); // 逐点绘制黄色水平线
    }

    screen_y = 60 + (69 - hang_max); // 转换为屏幕坐标（0=最下面，69=最上面）
    for (int x = 0; x < LCDW; x++) {
        ips200_draw_point(x, screen_y, RGB565_BROWN); // 逐点绘制黄色水平线
    }


    if(run_flag==0)
    {
        // 显示关键信息
        char info_str[50];

        sprintf(info_str, "err:%d", image_error);
        ips200_show_string(0, 0, info_str);

        sprintf(info_str, "R_black:%d", R_black[hang_max-1]);
        ips200_show_string(0, 20, info_str);

        sprintf(info_str, "L_black:%d", L_black[hang_max-1]);
        ips200_show_string(0, 35, info_str);

        sprintf(info_str, "end_hang:%d", hang_max);
        ips200_show_string(0, 150, info_str);

        sprintf(info_str, "servo_duty:%d", servo_duty);
        ips200_show_string(0, 165, info_str);
        
        sprintf(info_str, "wall_L:%d", wall_L);
        ips200_show_string(0, 180, info_str);

        sprintf(info_str, "wall_R:%d", wall_R);
        ips200_show_string(0, 195, info_str);
        sprintf(info_str, "Distance:%f", Distance);
        ips200_show_string(0, 210, info_str);
        sprintf(info_str, "L_i_f:%d", huandao_flag_left);
        ips200_show_string(0, 235, info_str);
        sprintf(info_str, "road_state:%d", road_state);
        ips200_show_string(0, 250, info_str);
        sprintf(info_str, "otsu_Min:%d", otsuMinThreshold);
        ips200_show_string(0, 265, info_str);
        sprintf(info_str, "otsu_Max:%d", otsuMaxThreshold);
        ips200_show_string(0, 280, info_str);
    }

}


void set_speed_choice(uint8_t index)
{
    if(index>=1 && index<=3)
    {
        if(index == 1)
        {
            huandao_speed = set_speed_low[0];
            lz_speed = set_speed_low[1];
            dz_speed = set_speed_low[2];
            wan_speed = set_speed_low[3];
            ramp_speed_up = set_speed_low[4]; 
            ramp_speed_down = set_speed_low[5];
            HIGH_SPEED_THRESHOLD = lz_speed-150; 
        }
        else if(index == 2)
        {
            huandao_speed = set_speed_mid[0];
            lz_speed = set_speed_mid[1];
            dz_speed = set_speed_mid[2];
            wan_speed = set_speed_mid[3];
            ramp_speed_up = set_speed_mid[4]; 
            ramp_speed_down = set_speed_mid[5];
            HIGH_SPEED_THRESHOLD = lz_speed-150; 
        }
        else if(index == 3)
        {
            huandao_speed = set_speed_high[0];
            lz_speed = set_speed_high[1];
            dz_speed = set_speed_high[2];
            wan_speed = set_speed_high[3];
            ramp_speed_up = set_speed_high[4]; 
            ramp_speed_down = set_speed_high[5];
            HIGH_SPEED_THRESHOLD = lz_speed-150; 
        }
    }
    return;
}

// 静态变量：用于小s标志的连续帧计数
int little_s_consecutive_frames = 0;

void setTargetSpeed(void) {
    uint64_t current_time = get_current_time_ms();
    speed_setPoint = lz_speed;
}

// 主图像处理函数
void ImageProcess()
{
    // 帧率计算相关变量
    static uint64_t frame_times[10] = {0}; // 存储最近10帧的时间戳
    static int frame_index = 0;           // 当前帧索引
    static bool buffer_filled = false;     // 是否已收集满10帧数据
    
    // 获取当前时间戳
    uint64_t current_time = get_current_time_ms();
    
    // 测量getCompressedImage耗时
    uint64_t start_time = get_current_time_ms();
    getCompressedImage();
    uint64_t getImage_time = get_current_time_ms() - start_time;

    // 出界保护
    if (out_fill_protect()&&run_flag)
    {
        break_flag = 1;
        std::cout << "*********chujie********" << std::endl;
        ips200_clear();
        ips200_show_string(0, 0, "chujie");
        protect();
    }

    // 测量Image_Process耗时
    start_time = get_current_time_ms();
    Image_Process();
    uint64_t imageProc_time = get_current_time_ms() - start_time;
    
    // 测量pot_judge耗时
    start_time = get_current_time_ms();
    uint64_t potJudge_time = get_current_time_ms() - start_time;
    
    // 测量GetDet耗时
    start_time = get_current_time_ms();
    GetDet();
    uint64_t getDet_time = get_current_time_ms() - start_time;

    // 更新帧时间缓冲区
    frame_times[frame_index] = current_time;
    frame_index = (frame_index + 1) % 10;
    if (frame_index == 0) buffer_filled = true;

    // 计算FPS (基于前10帧)
    //float fps = 0.0f;
    if (buffer_filled)
    {
        uint64_t time_span = current_time - frame_times[frame_index]; // 最新帧与最旧帧的时间差
        fps = 10000.0f / time_span; // 10帧 / (时间差/1000秒) = 10000/time_span
    }

    if(fps_flag == 1)
    {
        // 打印所有函数执行时间和FPS
        printf("FPS=%.1f img_index=%d \n", fps ,img_index);
        printf("Times: getImage=%llu imageProc=%llu potJudge=%llu getDet=%llu \n", 
            getImage_time, imageProc_time, potJudge_time, getDet_time);
    }

}
uint64_t delay_start;
int cheku_1=300;
void control(void){
    // 舵机控制
    //发车出车库
    //if(run_flag = 2)uint64_t run_start = get_current_time_ms();
    if(((get_current_time_ms()-delay_start)<=cheku_1)&&run_flag==1){
        servo_duty=4600;
    }
    //车库2侧向停车,靠Distance进行距离判断
    // else if((1)&&run_flag==1){

    // }


    //正常图像行驶
    else{
        pid_output = TurnPID_Calculate(image_error);
        servo_duty = std::max(-SERVO_PWM_MAX, std::min(SERVO_PWM_MAX, pid_output));
        servo_duty += servo_base;
        if(hang_max<=40 && ima_change==1 && Distance<=350){
            wall_L=0;
            wall_R=0;
            for(hang=hang_max;hang>0;hang--){
                if(BinPixels[hang][185]==0)wall_L++;
                else break;
            }
            for(hang=hang_max;hang>0;hang--){
                if(BinPixels[hang][0]==0)wall_R++;
                else break;
            }
            if(wall_L<wall_R){
                servo_duty=5500;
            }
            else if(wall_L>wall_R){
                servo_duty=4500;
            }
        }

        // if(R_black[hang_max-1]<93 && L_black[hang_max-1]<93 && abs(R_black[hang_max-1]-L_black[hang_max-1])<30 &&hang_max<=40){
        //     servo_duty=5500;
        //     //wall_first=1;
        // }
        // else if(R_black[hang_max-1]>93 && L_black[hang_max-1]>93 && abs(R_black[hang_max-1]-L_black[hang_max-1])<30 &&hang_max<=40){
        //     servo_duty=4500;
        // }
    }




    if(dj_flag == 1) pwm_set_duty(SERVO_PWM, (uint16_t)servo_duty);

    if(chasu_flag==2)
    {
        if(break_flag==0)
        {
            speed_setPoint_l = speed_setPoint + image_error * chasu_k; 
            speed_setPoint_r = speed_setPoint - image_error * chasu_k;
        }
        else
        {
            speed_setPoint_l = 0;
            speed_setPoint_r = 0;
        }
    }
    else 
    {
        speed_setPoint_l = speed_setPoint;
        speed_setPoint_r = speed_setPoint;
    }
}

void run_loop(void) {
    run_init();

    uint64_t last_protect_check = 0;  // 上次保护检查时间
    const uint64_t protect_check_interval = 100; // 保护检查间隔（毫秒）
    uint8_t last_banma_cnt = 0;       // 新增：上次banma_cnt值
    elapsed = 0;
    jishi_start_time = 0;     // 新增：斑马线计数开始时间
    dj_flag = 1;
    // 等待用户选择模式
    bool selection_done = false;

    ips200_clear();
    ips200_show_string(0, 0, "KEY_UP----->test");
    ips200_show_string(0, 20, "KEY_DOWN--->speed_test");
    ips200_show_string(0, 40, "KEY_enter--->banma");
    getCompressedImage();

    while (!selection_done) {
        char Key_Num = detect_key();
        
        if (Key_Num == 1) {
            banma_fache_flag = 0;
            speed_test_mode = 0;
            selection_done = true;
            ips200_show_string(0, 80, "test_fache");
            system_delay_ms(500);
        } 
        else if (Key_Num == 3) {
            banma_fache_flag = 1;
            speed_test_mode = 0;
            selection_done = true;
            ips200_show_string(0, 80, "banma_fache");
            system_delay_ms(500);            
        }
        else if (Key_Num == 2) {
            banma_fache_flag = 1;
            speed_test_mode = 1;
            selection_done = true;
            ips200_show_string(0, 80, "speed_test_mode");
            system_delay_ms(500);
        }
        else if (Key_Num == 4) {
            return;
        }
    }

    while (!should_exit && !exit_flag) {
        char Key_Num;
        Key_Num = detect_key();

        // 按键3处理
        if (Key_Num == 3) {
            if (run_flag == 0) {
                // 第一次按下
                ips200_clear();
                run_flag = 1;
                ips200_show_string(0, 0, "---fuya_init---");
                if (fy_flag) pwm_set_duty(PWM_1, fy_zhi);
                else 
                {
                    pwm_set_duty(PWM_1, 5000);
                }
                continue;
            } 
            else if (run_flag == 1) {
                // 第二次按下：启动系统
                ips200_clear();
                run_init();
                ips200_show_string(0, 0, "---waiting---");
                delay_start = get_current_time_ms();
                bool interrupted = false;
                
                while (get_current_time_ms() - delay_start < 1000 && !interrupted) {
                    char temp_key = detect_key();
                    if (temp_key == 4) {
                        interrupted = true;
                        run_flag = 0;
                        gpio_set_level(BEEP, 0x0);
                        pwm_set_duty(SERVO_PWM, 0);
                        pwm_set_duty(PWM_1, 5000);
                        run_init();
                        ips200_clear();
                        ips200_show_string(0, 0, "Operation canceled");
                        system_delay_ms(500);
                        break;
                    }
                }
                
                if (!interrupted) {
                    run_flag = 2;
                    ips200_show_string(0, 0, "***yunxing***");
                    all_start_distance = accumulated_distance; // 记录起始距离
                    all_start_time = get_current_time_ms();
                    printf("*********fache********\n");
                }
            }
        }

        // 按键4：停止系统
        if (Key_Num == 4) {
            run_flag = 0;
            gpio_set_level(BEEP, 0x0);
            pwm_set_duty(SERVO_PWM, 0);
            pwm_set_duty(PWM_1, 5000);
            run_init();
            break;
        }

        ImageProcess();
        control();

        if (run_flag == 0)
            drawBoundaryPoints();

        if (sd_flag == 1)
            saveTask();


        if(banma_cnt==banma_jieshu_cnt)
        {
            protect();
        }

        // 定时保护检查
        if (run_flag == 2) {
            uint64_t current_time = get_current_time_ms();
            during_time = (current_time - all_start_time);
            if (current_time - last_protect_check >= protect_check_interval) {
                last_protect_check = current_time;
                if ((current_time - all_start_time) >= time_lock && switch_baohu == 1) {
                    printf("*********time_over********\n");
                    break_flag = 1;
                    ips200_clear();
                    ips200_show_string(0, 0, "time_over");
                    protect();
                }
            }
        }

        uint64_t loop_end = get_current_time_ms();
    }

    // 退出清理
    pwm_set_duty(SERVO_PWM, 0);
    gpio_set_level(BEEP, 0x0);
    pwm_set_duty(PWM_1, 5000);
}

int out_fill_protect(void)
{
      uint8 i, j;
      for(i=0; i<=3; i++)
      {
       for(j=0; j<MT9V03X_W-1; j++)
       if(BinPixels[i][j]==0)
       {
          continue;
        }
       else
       {
         return 0;
       }
      }

      flag_break = 1;
      return 1;

}

void ImageInit()
{
    byte y;
    //将图像赋给BinPixels
    for (y = 0; y < 70; y++)
    {
        
        //将横向搜索数组及lcr结构体清零
        L_black[y] = 185;
        R_black[y] = 0;
        L_Start[y] = 0;
        R_Start[y] = 0;
        LCenter[y] = 0;
    }

}



void run_init(void) // 初始化函数
{
    run_flag=0;
    road_state = long_z;
    //控制参数
    speed_r_output=0;
    speed_l_output=0;
    run_flag = 0;
    img_index = 1;
    break_flag = 0;
    speed_r_integral = 0;
    speed_l_integral = 0;
    exit_flag=0;
    max_speed_l=0;
    max_speed_r=0;
    max_speed=0;
    //斑马线
    has_been_banmaxian = false;  // 标记是否已经经历过斑马线    
    banmaxian_flag = 0;
    banma_cnt = 0;
    //环岛
    huandao_flag_right = 0;
    huandao_flag_left = 0;
    //直弯
    cross_state_flag = 0;


//printf("[DEBUG] 准备修改 xie_shizi_flag_right (当前值=%d)\n", xie_shizi_flag_right);
xie_shizi_flag_right = 0;  // 你的赋值语句
//printf("[DEBUG] 修改完成，新值=%d\n", xie_shizi_flag_right);

    xie_shizi_flag_left = 0;
    //坡道
    ramp_flag=0;
    //扫线
    lie_xielv_use_begin = 0;
    lie_xielv_use_end = 26;

    //计算距离
    accumulated_distance = 0;
    
}

void cheku_2_start(){//R_black
    int qidian1=0,qi_flag=0,qidian2=0;
    qidian1=0;
    qi_flag=0;
    qidian2=0;
    for (hang = 0; hang < 5; hang++){
        if(R_black[hang]==0){
            qidian1++;
        }
    }
    if(qidian1==5){
        qi_flag=1;
    }

    if(qi_flag==1){
        for (hang = 0; hang < 70; hang++){
            if(R_black[hang]>0){
                break;
            }
        }
        for(hang;hang<70;hang++){
            if(R_black[hang]>0){
                qidian2++;
            }
            else if(R_black[hang]==0){
                break;
            }
        }
        if(qidian2>=5){

        }
    }


}

unsigned char image_change=0;
void Image_Process()
{
    ImageInit();
    //
    Find_Baseline_hang();
    saoxian_shu();
    //
    setTargetSpeed();
    
}


/************************************************************图像移植******************************************************************************************* */


        /*************辅助函数*************/
        int abs(int x)
        {
            if (x < 0) x = (int)-x;
            return x;

        }
        int myabs(int x)
        {
            return (x >= 0 ? x : -(x));
        }
        float jie_shu(float x, float n)//阶乘
        {
            float temp = x;
            for (byte i = 1; i < n; i++)
            {
                x *= temp;
            }
            return x;
        }
        void draw_curve(int type, int start, int end)
        {
            if (start <= 0) { start = 0; }
            if (end >= 69) { end = 69; }
            int k;
            int min = 0, max = 185;
            for (int i = end; i > start; i--)
            {
                k = (int)(curve_a * i * i + curve_b);
                if (k > max) k = max;
                if (k < min) k = min;
                if (type == 0)
                {
                    LCenter[i] = (byte)k;
                }
                if (type == 1)
                {
                    L_black[i] = (byte)k;
                }
                if (type == 2)
                {
                    R_black[i] = (byte)k;
                }
            }
        }
        void expect(int type, int startline, int endline)//求每行的左右边线期望值
        {
            if (type == 2)
            {
                int delta = startline - endline;
                if (startline <= 0) { startline = 0; }
                if (endline >= 69) { endline = 69; }
                if (delta == 0) delta = 1;
                float k = (R_black[startline] - R_black[endline]) * 1.0f / delta;
                float b = R_black[startline] - k * startline;
                for (int j = (byte)startline; j <= (byte)endline; j++)
                {
                    int jicun = ((int)(k * j + b));
                    if (jicun >= 185) jicun = 185;
                    else if (jicun <= 0) jicun = 0;
                    R_expect[j] = (byte)jicun;
                    //纠正右线
                }
            }
            if (type == 1)
            {
                int delta = startline - endline;
                if (startline <= 0) { startline = 0; }
                if (endline >= 69) { endline = 69; }
                if (delta == 0) delta = 1;
                float k = (L_black[startline] - L_black[endline]) * 1.0f / delta;
                float b = L_black[startline] - k * startline;
                for (int j = (byte)startline; j <= (byte)endline; j++)
                {
                    int jicun = ((int)(k * j + b));
                    if (jicun >= 185) jicun = 185;
                    else if (jicun <= 0) jicun = 0;
                    L_expect[j] = (byte)jicun;
                    //纠正右线
                }
            }
        }
        float fangcha(int type, int startline, int endline)//求方差 1-左 2-右 0-中
        {
            if (startline <= 0) { startline = 0; }
            if (endline >= 69) { endline = 69; }
            int i = 0;
            int sumlines = endline - startline;
            int sum = 0;
            float average = 0;

            if (type == 0)   //拟合中线
            {
                for (i = startline; i < endline; i++)
                {

                    sum += LCenter[i];
                }
                if (sumlines != 0)
                {
                    average = (float)sum / sumlines;     //x的平均值
                }
                else
                {
                    average = 0;     //x的平均值                       //y的平均值
                }
                for (i = startline; i < endline; i++)
                {
                    sumE += (LCenter[i] - average) * (LCenter[i] - average);
                }
                if (sumlines != 0)
                {
                    sumE = (sumE) / sumlines;
                    return sumE;
                }
                else sumE = 0;
            }
            else if (type == 1)//拟合左线
            {
                expect(1, startline, endline);

                for (i = startline; i < endline; i++)
                {
                    sum += (L_black[i] - L_expect[i]) * (L_black[i] - L_expect[i]);
                }
                if (sumlines != 0)
                {
                    sumE = (float)sum / sumlines;
                    return sumE;
                }
                else sumE = 0;
            }
            else if (type == 2)//拟合右线
            {
                expect(2, startline, endline);

                for (i = startline; i <= endline; i++)
                {
                    sum += (R_black[i] - R_expect[i]) * (R_black[i] - R_expect[i]);
                }
                if (sumlines != 0)
                {
                    sumE = (float)sum / sumlines;
                    return sumE;
                }
                else sumE = 0;
            }
            return 0;
        }
        void advanced_regression(int type, int startline1, int endline1, int startline2, int endline2)      //1为左 2为右 0为中
        {
            int i = 0;
            int sumlines1 = endline1 - startline1;
            int sumlines2 = endline2 - startline2;
            if (sumlines1 == 0 && sumlines2 == 0)
            {
                sumlines2 = 1;
            }
            int sumX = 0;
            int sumY = 0;
            float averageX = 0;
            float averageY = 0;
            float sumUp = 0;
            float sumDown = 0;
            if (type == 0)  //拟合中线
            {
                /**计算sumX sumY**/
                for (i = startline1; i < endline1; i++)
                {
                    sumX += i;
                    sumY += LCenter[i];
                }
                for (i = startline2; i < endline2; i++)
                {
                    sumX += i;
                    sumY += LCenter[i];
                }
                averageX = (float)(sumX / (sumlines1 + sumlines2));     //x的平均值
                averageY = (float)(sumY / (sumlines1 + sumlines2));     //y的平均值
                for (i = startline1; i < endline1; i++)
                {
                    sumUp += (LCenter[i] - averageY) * (i - averageX);
                    sumDown += (i - averageX) * (i - averageX);
                }
                for (i = startline2; i < endline2; i++)
                {
                    sumUp += (LCenter[i] - averageY) * (i - averageX);
                    sumDown += (i - averageX) * (i - averageX);
                }
                if (sumDown == 0) parameterB = 0;
                else parameterB = sumUp / sumDown;
                parameterA = averageY - parameterB * averageX;

            }
            else if (type == 1)     //拟合左线
            {
                /**计算sumX sumY**/
                for (i = startline1; i < endline1; i++)
                {
                    sumX += i;
                    sumY += L_black[i];
                }
                for (i = startline2; i < endline2; i++)
                {
                    sumX += i;
                    sumY += L_black[i];
                }
                averageX = (float)(sumX / (sumlines1 + sumlines2));     //x的平均值
                averageY = (float)(sumY / (sumlines1 + sumlines2));     //y的平均值
                for (i = startline1; i < endline1; i++)
                {
                    sumUp += (L_black[i] - averageY) * (i - averageX);
                    sumDown += (i - averageX) * (i - averageX);
                }
                for (i = startline2; i < endline2; i++)
                {
                    sumUp += (L_black[i] - averageY) * (i - averageX);
                    sumDown += (i - averageX) * (i - averageX);
                }
                if (sumDown == 0) parameterB = 0;
                else parameterB = sumUp / sumDown;
                parameterA = averageY - parameterB * averageX;
            }
            else if (type == 2)         //拟合右线
            {
                /**计算sumX sumY**/
                for (i = startline1; i < endline1; i++)
                {
                    sumX += i;
                    sumY += R_black[i];
                }
                for (i = startline2; i < endline2; i++)
                {
                    sumX += i;
                    sumY += R_black[i];
                }
                averageX = (float)(sumX / (sumlines1 + sumlines2));     //x的平均值
                averageY = (float)(sumY / (sumlines1 + sumlines2));     //y的平均值
                for (i = startline1; i < endline1; i++)
                {
                    sumUp += (R_black[i] - averageY) * (i - averageX);
                    sumDown += (i - averageX) * (i - averageX);
                }
                for (i = startline2; i < endline2; i++)
                {
                    sumUp += (R_black[i] - averageY) * (i - averageX);
                    sumDown += (i - averageX) * (i - averageX);
                }
                if (sumDown == 0) parameterB = 0;
                else parameterB = sumUp / sumDown;
                parameterA = averageY - parameterB * averageX;
            }
            if (parameterB >= 100) parameterB = 100;
            else if (parameterB <= -100) parameterB = -100;
        }
        void my_advanced_regression(int type, int startline, int endline)
        {
            int i = 0;
            int sumlines = endline - startline;
            if (sumlines == 0)
            {
                sumlines = 1;
            }
            int sumX = 0;
            int sumY = 0;
            float averageX = 0;
            float averageY = 0;

            if (type == 0)  //拟合中线
            {
                /**计算sumX sumY**/
                for (i = startline; i < endline; i++)
                {
                    sumX += i;
                    sumY += LCenter[i];
                }

                averageX = (float)((float)sumX / sumlines);     //x的平均值
                averageY = (float)((float)sumY / sumlines);     //y的平均值
                float x_use_startline = (startline - averageX) > -0.01f ? -0.01f : (startline - averageX);
                float x_use_endline = (endline - averageX) < 0.01f ? 0.01f : (endline - averageX);
                parameterB = ((LCenter[startline] - averageY) / x_use_startline / 2.0f + (LCenter[endline] - averageY) / x_use_endline / 2.0f);
                parameterA = averageY - parameterB * averageX;

            }



            if (type == 1)  //拟合左线
            {
                /**计算sumX sumY**/
                for (i = startline; i < endline; i++)
                {
                    sumX += i;
                    sumY += L_black[i];
                }

                averageX = (float)((float)sumX / sumlines);     //x的平均值
                averageY = (float)((float)sumY / sumlines);     //y的平均值
                float x_use_startline = (startline - averageX) > -0.01f ? -0.01f : (startline - averageX);
                float x_use_endline = (endline - averageX) < 0.01f ? 0.01f : (endline - averageX);
                parameterB = ((L_black[startline] - averageY) / x_use_startline / 2.0f + (L_black[endline] - averageY) / x_use_endline / 2.0f);
                parameterA = averageY - parameterB * averageX;

            }




            if (type == 2)  //拟合右线
            {
                /**计算sumX sumY**/
                for (i = startline; i < endline; i++)
                {
                    sumX += i;
                    sumY += R_black[i];
                }
                averageX = (float)((float)sumX / sumlines);     //x的平均值
                averageY = (float)((float)sumY / sumlines);     //y的平均值
                float x_use_startline = (startline - averageX) > -0.01f ? -0.01f : (startline - averageX);
                float x_use_endline = (endline - averageX) < 0.01f ? 0.01f : (endline - averageX);
                parameterB = ((R_black[startline] - averageY) / x_use_startline / 2.0f + (R_black[endline] - averageY) / x_use_endline / 2.0f);
                parameterA = averageY - parameterB * averageX;

            }



            if (parameterB >= 100) parameterB = 100;
            else if (parameterB <= -100) parameterB = -100;
        }

        //拟合直线函数
        void draw_line(int type, int startline, int endline, float parameterB, float parameterA)
        {
            if (startline <= 0) { startline = 0; }
            if (endline >= 69) { endline = 69; }
            int num;
            if (type == 1)              //左
            {
                for (int i = startline; i < endline; i++)
                {
                    num = (int)(parameterB * i + parameterA);
                    if (num < 0)
                    {
                        num = 0;
                    }
                    if (num > 185)
                    {
                        num = 185;
                    }
                    L_black[i] = (byte)num;
                }
            }
            else if (type == 2)            //右
            {
                for (int i = startline; i < endline; i++)
                {
                    num = (int)(parameterB * i + parameterA);
                    if (num < 0)
                    {
                        num = 0;
                    }
                    if (num > 185)
                    {
                        num = 185;
                    }
                    R_black[i] = (byte)num;
                }
            }
            else if (type == 0)             //中
            {
                for (int i = startline; i < endline; i++)
                {
                    num = (int)(parameterB * i + parameterA);
                    if (num < 0)
                    {
                        num = 0;
                    }
                    if (num > 185)
                    {
                        num = 185;
                    }
                    LCenter[i] = (byte)num;

                }
            }
        }
        //这个函数的主要作用是在两点之间拉线
        void draw_line_two(int x1, int y1, int x2, int y2, int type)
        {
            //保证y1<=y2
            if (y1 == y2)
            {
                //纵坐标不可以相同
                return;
            }
            if (y1 > y2)
            {
                int tmp;

                tmp = x1;
                x1 = x2;
                x2 = tmp;

                tmp = y1;
                y1 = y2;
                y2 = tmp;
            }
            if (x1 == x2)
            {
                for (int i = y1; i <= y2; i++)
                {
                    if (type == 1)
                    {
                        if (x1 <= 0)
                            x1 = 0;
                        else if (x1 >= 185)
                            x1 = 185;
                        L_black[i] = (byte)x1;
                    }
                    else if (type == 2)
                    {
                        if (x2 <= 0)
                            x2 = 0;
                        else if (x2 >= 185)
                            x2 = 185;
                        R_black[i] = (byte)x1;
                    }
                }
                return;
            }

            float k = (float)(x2 - x1) / (float)(y2 - y1);
            float b = (float)(x1 - k * y1);
            for (int i = y1; i <= y2; i++)
            {
                if (type == 1)
                {
                    if (k * i + b >= 186)
                    {
                        L_black[i] = 186 - 1;
                    }
                    else if (k * i + b < 0)
                    {
                        L_black[i] = 0;
                    }
                    else
                    {
                        L_black[i] = (byte)(k * i + b);
                    }
                }
                else if (type == 2)
                {
                    if (k * i + b >= 186)
                    {
                        R_black[i] = 186 - 1;
                    }
                    else if (k * i + b < 0)
                    {
                        R_black[i] = 0;
                    }
                    else
                    {
                        R_black[i] = (byte)(k * i + b);
                    }
                }

            }
        }

        void judge_duanhang(byte startline, byte endline, byte type)// 1-判断左线断行 2-判断右线断行
        {
            if (startline <= 0) { startline = 0; }
            if (endline >= 69) { endline = 69; }
            hang = 0;
            if (type == 1)
            {
                left_break = 0;
                for (hang = startline; hang < endline; hang++)
                {
                    if (L_black[hang] > 180) { left_break = hang; }
                    else break;
                }
            }
            else if (type == 2)
            {
                right_break = 0;
                for (hang = startline; hang < endline; hang++)
                {
                    if (R_black[hang] < 5) { right_break = hang; }
                    else break;
                }
            }
        }
        void judge_dh(byte starthang, byte endhang, byte type)// 1-判断左线断行 2-判断右线断行(防轻微黑腐蚀版本)
        {
            hang = 0;
            if (type == 1)
            {
                left_break = 0;
                for (hang = starthang; hang < endhang; hang++)
                {
                    if (L_black[hang] >= 180) { left_break = hang; }
                }
            }
            else if (type == 2)
            {
                right_break = 0;
                for (hang = starthang; hang < endhang; hang++)
                {
                    if (R_black[hang] <= 5) { right_break = hang; }
                }
            }
        }
        float my_fabs(float num)//求浮点类数据绝对值
        {
            if (num < 0.0f) return -num;
            else return num;
        }
        float my_sqrt(float a)//求浮点类数据平方根
        {
            float x = a, y = 0.0F;
            while (my_fabs(x - y) > 0.01)
            {
                y = x;
                x = (float)(0.5 * (x + a / x));
            }
            if (x > 185) x = 185;
            return x;
        }
        
        /*************这里写图像处理*************/
        void Find_Baseline_use()
        {
            int len_max = 0;
            byte s_line = 0;//start起始
            byte e_line = 0;//end终止
            byte cm_line = 0;
            byte lianxu_flag = 0;
            byte sm_line = 0;
            byte em_line = 0;
            for (lie = 0; lie < 185; lie++)//找第一行赛道最长连续白条的起点和终点
            {
                if (BinPixels[1][lie] != 0)
                {
                    e_line = (byte)lie;
                    if (lianxu_flag == 0) { s_line = (byte)lie; lianxu_flag = 1; }
                    if (e_line - s_line > em_line - sm_line)
                    {
                        em_line = e_line;
                        sm_line = s_line;
                    }
                }
                else
                {
                    lianxu_flag = 0;
                }
            }
            sm_line = sm_line <= 10 ? (byte)10 : sm_line;
            em_line = em_line >= 175 ? (byte)175 : em_line;
            cm_line = (byte)(sm_line / 2 + em_line / 2);
            cm_line = cm_line >= 135 ? (byte)135 : cm_line;
            cm_line = cm_line <= 50 ? (byte)50 : cm_line;
            ////SetText("sm_line = " + sm_line);
            ////SetText("em_line = " + em_line);
            ////SetText("cm_line = " + cm_line);
            for (lie = (byte)(cm_line - 50); lie < cm_line + 50; lie++)//从右往左找白色最长列
            {
                for (hang = 0; hang < 68; hang++)
                {
                    if (BinPixels[hang + 1][lie] == 0) break;//直到扫到黑色结束
                }
                if (hang > len_max)
                {
                    baseline_use = lie;
                    len_max = hang;
                }
            }
            ////SetText("baseline_use = " + baseline_use);
        }
        byte Baseline_Findline_use()
        {
            byte lie_left = 185;
            byte lie_right = 0;
            for (lie = baseline_use; lie >= 1; lie--)//找右边界
            {
                if (lie >= 185) lie = 184;
                if (BinPixels[0][lie - 1] == 0 && BinPixels[0][lie] == 0 && BinPixels[0][lie + 1] != 0) //白黑黑
                {
                    lie_right = (byte)(lie);
                    break;
                }
                else//右边断线
                {
                    lie_right = 0;
                }
            }
            ////SetText("lie_right = " + lie_right);
            for (lie = baseline_use; lie < 185; lie++)//找左边界
            {
                if (lie == 0) lie = 1;
                if (BinPixels[0][lie + 1] == 0 && BinPixels[0][lie] == 0 && BinPixels[0][lie - 1] != 0)//白黑黑
                {
                    lie_left = (byte)(lie);
                    break;
                }
                else//左边断线
                {
                    lie_left = 185;
                }
            }
            ////SetText("lie_left = " + lie_left);
            return (byte)(lie_left / 2 + lie_right / 2);
        }
        void Find_Baseline_hang()
        {
            lie = 0;
            hang = 0;
            byte xielv_cnt = 0; int hang_len_max = 0; int hang_len = 0;
            Find_Baseline_use();
            qidian = Baseline_Findline_use();
            if (qidian < 2) { qidian = 2; }
            if (qidian > 184) { qidian = 184; }
            //SetText("qidian = " + qidian);

            for (lie = qidian; lie < 184; lie++)   //左
            {
                if (BinPixels[0][lie] == 0 && BinPixels[0][lie - 1] == 0 && BinPixels[0][lie - 2] != 0)
                {
                    L_black[0] = (byte)(lie - 2);
                    break;
                }
                else
                {
                    L_black[0] = 185;
                }

            }
            for (lie = qidian; lie > 2; lie--)      //右
            {
                if (BinPixels[0][lie] == 0 && BinPixels[0][lie + 1] == 0 && BinPixels[0][lie + 2] != 0)
                {
                    R_black[0] = (byte)(lie + 2);
                    break;
                }
                else
                {
                    R_black[0] = 0;
                }
            }
            if (L_black[0] > 175)
                L_black[0] = 175;

            if (R_black[0] < 10)
                R_black[0] = 10;
            start_lie = R_black[0];
            last_lie = L_black[0];

            lie_zhong = (byte)((start_lie + last_lie) / 2);

            for (byte x_i_hang = 0; x_i_hang < 26; x_i_hang++)
            {
                for (byte y_i_hang = 0; y_i_hang < 70; y_i_hang++)
                {
                    reference_lie[x_i_hang][y_i_hang] = 0;
                }
            }
            ////SetText("基准线_左边界" + last_lie); //SetText("基准线_右边界" + start_lie);
            ////SetText("斜基准线起点" + lie_zhong);
            for (xielv_cnt = lie_xielv_use_begin; xielv_cnt < lie_xielv_use_end; xielv_cnt++)
            {
                reference_lie[xielv_cnt][0] = (byte)lie_zhong;
                if (BinPixels[0][reference_lie[xielv_cnt][0]] != 0)
                {
                    for (hang = 1; hang < 70; hang++)
                    {
                        if (lie_xielv[xielv_cnt] != 0)
                        {
                            lie = (int)(((double)hang / lie_xielv[xielv_cnt] + (double)lie_zhong) < 0 ? 0 : ((double)hang / lie_xielv[xielv_cnt] + (double)lie_zhong));
                        }
                        if (lie_xielv[xielv_cnt] == 0)
                        {
                            lie = lie_zhong;
                        }

                        if (lie > 185)
                        {
                            lie = 185;

                        }
                        if (lie < 0)
                        {
                            lie = 0;

                        }
                        reference_lie[xielv_cnt][hang] = (byte)lie;
                        if ((BinPixels[hang][lie] == 0 || hang == 69 || lie == 184 || lie == 1))
                        {

                            {
                                hang_len = (int)(myabs(lie - lie_zhong) * myabs(lie - lie_zhong) + (double)hang * (double)hang * (double)30);
                            }

                            break;
                        }
                        else
                        {
                            hang_len = 0;
                        }

                    }
                    if (hang_len > hang_len_max)
                    {
                        hang_len_max = hang_len;
                        xielv_use = xielv_cnt;
                        hang_max = (byte)(hang);
                        hang_len = 0;
                    }
                }
            }
        }
        double hang_xielv_x_f(double hang_xielv)
        {
            double hang_xielv_max = 100;
            double hang_xielv_min = -100;
            if (hang_xielv > hang_xielv_max)
            {
                hang_xielv = hang_xielv_max;
            }

            if (hang_xielv < hang_xielv_min)
            {
                hang_xielv = hang_xielv_min;
            }

            return hang_xielv;
        }

        const float straight_standard = 4.0f;
        
        void really_fresh_line()
        {
            for (byte hang = 0; hang < 70; hang++)
            {
                L_black[hang] = 185;
                R_black[hang] = 0;
                if (hang < 10)
                {
                    record_leftline_begin[hang] = 0;
                    record_leftline_end[hang] = 0;
                    record_rightline_begin[hang] = 0;
                    record_rightline_end[hang] = 0;
                }
            }

        }

        void saoxian_shu()
        {
            really_fresh_line();
            for (hang = 0; hang < hang_max; hang++)
            {
                for (lie = reference_lie[xielv_use][hang]; lie < 185; lie++)   //左
                {
                    if (lie > 2)
                    {
                        if (BinPixels[hang][lie] == 0 && BinPixels[hang][lie - 1] == 0 && BinPixels[hang][lie - 2] != 0)
                        {
                            L_black[hang] = (byte)(lie - 2);
                            break;
                        }
                    }
                    else if (lie == 2)
                    {
                        if (BinPixels[hang][lie] == 0 && BinPixels[hang][lie - 1] == 0 && BinPixels[hang][lie - 2] != 0)
                        {
                            L_black[hang] = (byte)(lie - 2);
                            break;
                        }
                    }
                }
                for (lie = reference_lie[xielv_use][hang]; lie > 0; lie--)      //右
                {
                    if (lie < 182)
                    {
                        if (BinPixels[hang][lie] == 0 && BinPixels[hang][lie + 1] == 0 && BinPixels[hang][lie + 2] != 0)
                        {
                            R_black[hang] = (byte)(lie + 2);
                            break;
                        }
                    }
                    else if (lie == 182)
                    {
                        if (BinPixels[hang][lie] == 0 && BinPixels[hang][lie + 1] == 0 && BinPixels[hang][lie + 2] != 0)
                        {
                            R_black[hang] = (byte)(lie + 2);
                            break;
                        }
                    }
                }
                LCenter[hang] = (byte)((L_black[hang] + R_black[hang]) / 2);
            }
            byte lost_lineleft_begin = 0;
            byte lost_lineleft_end = 0;
            byte lost_lineright_begin = 0;
            byte lost_lineright_end = 0;
            byte lost_lineleft_cnt = 0;
            byte lost_lineright_cnt = 0;
            for (hang = 0; hang < hang_max - 1; hang++)
            {
                //left  断线行
                if (L_black[hang] == 185 && L_black[hang + 1] == 185 && lost_lineleft_begin == 0)
                {
                    lost_lineleft_begin = hang;
                    continue;
                }
                if (lost_lineleft_begin != 0 && L_black[hang] == 185)
                {

                    if (L_black[hang + 1] != 185 || hang + 1 >= (hang_max - 2))
                    {
                        lost_lineleft_end = hang;
                        record_leftline_begin[lost_lineleft_cnt] = lost_lineleft_begin;
                        record_leftline_end[lost_lineleft_cnt] = lost_lineleft_end;
                        lost_lineleft_cnt++;
                        if (lost_lineleft_cnt >= 10)
                            lost_lineleft_cnt = 9;
                        //SetText("左丟线行" + lost_lineleft_begin + "---" + lost_lineleft_end);
                        //SetText("左丟线行end" + lost_lineleft_end + "---" + L_black[record_leftline_end[0] + 1]);
                        lost_lineleft_begin = 0;
                        lost_lineleft_end = 0;
                        if (hang + 1 >= (hang_max - 2))
                        {
                            break;
                        }
                    }
                }
            }

            for (hang = 0; hang < hang_max - 1; hang++)
            {
                //right  断线行
                if (R_black[hang] == 0 && R_black[hang + 1] == 0 && lost_lineright_begin == 0)
                {
                    lost_lineright_begin = hang;
                    continue;
                }
                if (lost_lineright_begin != 0 && R_black[hang] == 0)
                {
                    if (R_black[hang + 1] != 0 || hang + 1 >= (hang_max - 2))
                    {
                        lost_lineright_end = hang;
                        record_rightline_begin[lost_lineright_cnt] = lost_lineright_begin;
                        record_rightline_end[lost_lineright_cnt] = lost_lineright_end;
                        lost_lineright_cnt++;
                        if (lost_lineright_cnt >= 10)
                            lost_lineright_cnt = 9;
                        //SetText("右丟线行" + lost_lineright_begin + "---" + lost_lineright_end);
                        lost_lineright_begin = 0;
                        lost_lineright_end = 0;
                        if (hang + 1 >= (hang_max - 2))
                        {
                            break;
                        }
                    }
                }
            }
            if (zr_shizi_flag == 0 && xie_shizi_flag_left == 0 && xie_shizi_flag_right == 0)
            {
                for (hang = hang_max; hang < 69; hang++)
                {
                    if (hang_max >= 2)
                    {
                        LCenter[hang] = LCenter[hang_max - 1];
                        //LCenter[hang - 1] = LCenter[hang_max - 2];
                    }
                    else
                        LCenter[hang] = LCenter[0];
                }
            }
            else
            {
                for (hang = 0; hang < 65; hang++)//从断线行开始进行归线
                {
                    if (hang <= 65 && hang >= 2 && banmaxian_flag == 0) //归线
                    {
                        if ((BinPixels[hang + 1][LCenter[hang]] == 0 || hang == hang_max) && LCenter[hang - 2] >= 93)
                        {
                            for (; hang < 70; hang++)
                            {
                                LCenter[hang] = 185;
                            }
                            break;
                        }
                        else if ((BinPixels[hang + 1][LCenter[hang]] == 0 || hang == hang_max) && LCenter[hang - 2] < 93)
                        {
                            for (; hang < 70; hang++)
                            {
                                LCenter[hang] = 0;
                            }
                            break;
                        }
                    }
                }
            }
        }
